#include <rtthread.h>
#include <sensor_common.h>
#include <rthw.h>
#include <attitude_estimate.h>
#include <stdio.h>
#include <stdarg.h>
#include <communication_common.h>

#include "misc.h"
#include "bsp_led.h"
#include "control.h"
static void reboot(rt_uint8_t argc, char **argv)
{
    rt_hw_cpu_reset();
}
MSH_CMD_EXPORT(reboot, "Reboot System");

int main()
{
    rt_kprintf("Main Thread Start!!!\n");

    start_sensor_thread();
    start_attitude_estimate();
    start_communication_thread();
    start_control_thread();

    struct attitude_euler euler = {0};
    struct attitude_euler_angular euler_angular = {0};

    while (1)
    {
        bsp_led_operation(led0, on);
        rt_thread_mdelay(200);
        bsp_led_operation(led0, off);
        rt_thread_mdelay(200);

        get_attitude(&euler, &euler_angular);
        rt_kprintf("Euler: [%d, %d, %d]; Angular: [%d, %d, %d]\n",
                   (int)(euler.pitch * RAD2DEG), (int)(euler.roll * RAD2DEG), (int)(euler.yaw * RAD2DEG),
                   (int)(euler_angular.pitch_angular * RAD2DEG), (int)(euler_angular.roll_angular * RAD2DEG), (int)(euler_angular.yaw_angular * RAD2DEG));
        // struct sbus_frame sbus;
        // sbus_frame_get(&sbus);
        // sbus_print_frame(&sbus);
    }

    return 0;
}